%% Oded script


%% Create detector
if ~exist('pedDetector')
   cd(fileparts(which('acfDemoCal.m'))); dataDir='../../data/Caltech/';
    for s=1:2
      if(s==1), type='test'; skip=[]; else type='train'; skip=4; end
      dbInfo(['Usa' type]); if(s==2), type=['train' int2str2(skip,2)]; end
      if(exist([dataDir type '/annotations'],'dir')), continue; end
      dbExtract([dataDir type],1,skip);
    end

    %%set up opts for training detector (see acfTrain)
    opts=acfTrain(); opts.modelDs=[50 20.5]; opts.modelDsPad=[64 32];
    opts.pPyramid.pChns.pColor.smooth=0; opts.nWeak=[64 256 1024 4096];
    opts.pBoost.pTree.maxDepth=5; opts.pBoost.discrete=0;
    opts.pBoost.pTree.fracFtrs=1/16; opts.nNeg=25000; opts.nAccNeg=50000;
    opts.pPyramid.pChns.pGradHist.softBin=1; opts.pJitter=struct('flip',1);
    opts.posGtDir=[dataDir 'train' int2str2(skip,2) '/annotations'];
    opts.posImgDir=[dataDir 'train' int2str2(skip,2) '/images'];
    opts.pPyramid.pChns.shrink=2; opts.name='models/AcfCaltech+';
    pLoad={'lbls',{'person'},'ilbls',{'people'},'squarify',{3,.41}};
    opts.pLoad = [pLoad 'hRng',[50 inf], 'vRng',[1 1] ];

    %%optionally switch to LDCF version of detector (see acfTrain)
    if( 0 ), opts.filters=[5 4]; opts.name='models/LdcfCaltech'; end

    %%train detector (see acfTrain)
    pedDetector = acfTrain( opts );

    %%modify detector (see acfModify)
    pModify=struct('cascThr',-1,'cascCal',.025);
    pedDetector=acfModify(pedDetector,pModify); 
end

%% Load video
if ~exist('vid')
    vid = VideoReader('Right.mpg');
    Frames = read(vid);
else
    fprintf('Using preloaded movie "%s"\n',vid.Name);
end

%% Run detector
clearvars -except Frames vid pedDetector
shapeInserter = vision.ShapeInserter('Shape','Rectangles','BorderColor','Custom', 'CustomBorderColor', uint8([255 0 0]));
rec = int32([50 60 100 60]); 

s = size(Frames,4);
bbs_y_av = [];
people_map = [];
people_bank = [];
rue = 20;
flag=0;
Threshold = 120;

for i=1:s%which frames
    I = imresize(Frames(:,:,:,i),[480 640]);
    tic 
    bbs=acfDetect(I,pedDetector);
    %Without KF at all%
    bbs1 = [];
    for b = 1:size(bbs,1)
        if bbs(b,5) > Threshold
            bbs1 = [bbs1; bbs(b,:)];
        else 
            break;
        end
        
    end
    %Current people in bank
%     for p=people_bank
%         if p.ass > Threshold
%             rec = int32([p.X(1),p.X(2),p.rec_size']);
%             I = step(shapeInserter, I, rec);    
%         end
%     end
%     figure(1); im(I);bbApply('draw',bbs1);
    
    %////begining of KF on people in bank and add new ///%
    %On all people in the bank perform KF propogation 
    temp_bank = [];
    for p=1:length(people_bank)
        flag=0;
        %Propogate the person
        people_bank(p) = people_bank(p).KF_propagate();
        
        %remove dead people from the bank
        if((people_bank(p).X(1)+people_bank(p).rec_size(1)) < 0 ...
            || (people_bank(p).X(2)+people_bank(p).rec_size(2)) < 0 ...
            || people_bank(p).X(1) > 480 || people_bank(p).X(2) > 640)
            continue;
        end
        temp_bank = [temp_bank people_bank(p)];
    end
    people_bank = temp_bank;
    %Update people in bank and add new people to bank
    for b = 1:size(bbs,1)
        if bbs(b,5) > Threshold% & bbs(b,2) > 100 & bbs(b,2) < 300
            found = 0;
            for p=1:length(people_bank)
                if people_bank(p).is_close_to_me(bbs(b,1:2),rue) %update person
                    people_bank(p) = people_bank(p).KF_update(bbs(b,1:4));
                    people_bank(p) = people_bank(p).update_assurance(bbs(b,5));
                    found  = 1;
                    break;
                end
            end
            if ~found %create a new person
                people_bank = [people_bank person(bbs(b,:))];
            end
        else
            break;
        end
    end
    
    %On all people in the bank perform assurance update if not found
    for p=1:length(people_bank)
        if ~people_bank(p).found
            people_bank(p) = people_bank(p).update_assurance(30);
        end
    end
               
    toc
    %////// end of KF process ////// %
    figure(1);subplot(2,1,1);im(I);bbApply('draw',bbs1);
    title(num2str(size(bbs1,1)));
    per = 0;
    per_tot = 0;
    for p=people_bank
        per_tot = per_tot + 1;
        if p.ass > Threshold
            rec = int32([p.X(1),p.X(2),p.rec_size']);
            I = step(shapeInserter, I, rec);    
            per = per + 1;
        end
    end
    subplot(2,1,2);im(I); %pause(0.5);
    title(['showing ',num2str(per),' people out of ',num2str(per_tot),' people in bank']);
end